#include "GenUtils.cpp"

bool SHOULD_DRAW;
bool TESTING;  //Will activate if a 'current image' is given from command line
int XDIM;
int YDIM;
int N_GOAL_LINES;
int N_CURR_LINES;


//Socket helpers:

void sigchld_handler(int s)
{
	while(waitpid(-1, NULL, WNOHANG) > 0);
}

// get sockaddr, IPv4 or IPv6:
void *get_in_addr(struct sockaddr *sa)
{
	if (sa->sa_family == AF_INET) {
		return &(((struct sockaddr_in*)sa)->sin_addr);
	}

	return &(((struct sockaddr_in6*)sa)->sin6_addr);
}

/*****************************************
\\\\\\\\\\\\\\\\\\| MAIN |\\\\\\\\\\\\\\\\\\
*******************************************/

int main( int argc, char** argv )
{
    if(argc < 2){
     cout <<" Usage: ./smmr goalImg (opt: currImg) (opt: should_draw)" << endl;
     return -1;
    }

    if(argc > 2){
        TESTING = 1;
        cout << "In testing mode - no socket will be opened." << endl;
    } else TESTING = 0;

    if(argc == 4) SHOULD_DRAW = atoi(argv[3]);
    else SHOULD_DRAW = 0;
//******************~ Prepare Logging ~**********************//

    char log_path[100];
    int t_start = clock();
    time_t rawtime;
    struct tm * timeinfo;
    time (&rawtime);
    timeinfo = localtime (&rawtime);  //TODO: do this better

    sprintf (log_path, "../outputImgs/%s/", asctime(timeinfo));    
    mkdir(log_path, 0777);

//*******************~ Image reading ~***********************//

    Mat I_g, I_c, cleanGoal, cleanCurr;
    I_g = imread(argv[1]);	// Read the file
    if(! I_g.data ){			// Check for invalid input
        cout <<  "Could not open or find the goal image" << endl ;
        return -1;
    }
    cleanGoal = I_g.clone();
    //Set these global variables
    XDIM = I_g.cols;
    YDIM = I_g.rows;

    if(TESTING){    
        I_c = imread(argv[2]);	// Read the file
        if(! I_c.data ){			// Check for invalid input
            cout <<  "Could not open or find the current image" << endl ;
            return -1;
        }
        cleanCurr = I_c.clone();
        if( XDIM != I_c.cols || YDIM != I_c.rows){
            cout <<  "Image dimensions don't match" << endl ;
            return -1;
        }
    }
    cout << "Images read..." << endl;

//*****************~ Parse Goal image ~**********************//	 

    LSR *LSR_g = new LSR[XDIM*YDIM/15]; // if we make minimum 15 cells, this will be max # LSRs;
    N_GOAL_LINES = findLSRs(LSR_g, I_g);
    Line *Lines_g = new Line[N_GOAL_LINES];
    linesFromLSRs(Lines_g, LSR_g, 1);    

    LineGroup L_g (Lines_g, 1);
    delete[] LSR_g;

    drawL(L_g, log_path, cleanGoal, 1);

    cout << "Goal image parsed..." << endl;

//********************~ Open Socket ~************************//

//*
/* Please note that the majority of this socket code came from "Beej's Guide to Network
/* Programming" (available at beej.us/guide/bgnet/), by Brian "Beej Jorgensen" Hill.
*/

    int new_fd, numbytes;
    char buf[MAXDATASIZE];
    if(!TESTING){
        int sockfd;  // listen on sock_fd
        struct addrinfo hints, *servinfo, *p;
        struct sockaddr_storage their_addr; // connector's address information
        socklen_t sin_size;
        struct sigaction sa;
        int yes=1;
        char s[INET6_ADDRSTRLEN];
        int rv;

        memset(&hints, 0, sizeof hints);
        hints.ai_family = AF_UNSPEC;
        hints.ai_socktype = SOCK_STREAM;
        hints.ai_flags = AI_PASSIVE; // use my IP

        if ((rv = getaddrinfo(NULL, PORT, &hints, &servinfo)) != 0) {
            fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
            return 1;
        }

        // loop through all the results and bind to the first we can
        for(p = servinfo; p != NULL; p = p->ai_next) {
            if ((sockfd = socket(p->ai_family, p->ai_socktype,
                    p->ai_protocol)) == -1) {
                perror("server: socket");
                continue;
            }

            if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &yes,
                    sizeof(int)) == -1) {
                perror("setsockopt");
                exit(1);
            }

            if (bind(sockfd, p->ai_addr, p->ai_addrlen) == -1) {
                close(sockfd);
                perror("server: bind");
                continue;
            }

            break;
        }

        if (p == NULL)  {
            fprintf(stderr, "server: failed to bind\n");
            return 2;
        }

        freeaddrinfo(servinfo); // all done with this structure

        if (listen(sockfd, BACKLOG) == -1) {
            perror("listen");
            exit(1);
        }

        sa.sa_handler = sigchld_handler; // reap all dead processes
        sigemptyset(&sa.sa_mask);
        sa.sa_flags = SA_RESTART;
        if (sigaction(SIGCHLD, &sa, NULL) == -1) {
            perror("sigaction");
            exit(1);
        }

        printf("server: waiting for connections...\n");

        sin_size = sizeof their_addr;
        new_fd = accept(sockfd, (struct sockaddr *)&their_addr, &sin_size);
        if (new_fd == -1) {
            perror("accept");
        }

        inet_ntop(their_addr.ss_family, get_in_addr((struct sockaddr *)&their_addr),
            s, sizeof s);
        printf("server: got connection from %s\n", s);
    }

//***************~ Starting main while loop ~****************//

int loopcounter = 0;
while(1){

    //*****************~ Update logging path ~**********************//

        sprintf (log_path, "../outputImgs/%s/%d/", asctime(timeinfo), loopcounter);
        mkdir(log_path, 0777);

    //****************~ Wait for ready signal ~******************//

        if(!TESTING){
            if ((numbytes = recv(new_fd, buf, MAXDATASIZE-1, 0)) == -1) {
                perror("recv");
                exit(1);
            }
            buf[numbytes] = '\0';
            printf("server: received '%s'\n",buf);

    //********************~ Current image ~**********************//

            //TODO: get the current image (I_c) from Camera denseFlow
            I_c = imread("../testImgs/depth_4.0.jpg");	// Read the file
            if(! I_c.data ){			// Check for invalid input
                cout <<  "Could not open or find the current image" << endl ;
                return -1;
            }
            cleanCurr = I_c.clone();
            if( XDIM != I_c.cols || YDIM != I_c.rows){
                cout <<  "Image dimensions don't match" << endl ;
                return -1;
            }
        }

        LSR *LSR_c = new LSR[XDIM*YDIM/15]; // if we make minimum 15 cells, this will be max # LSRs;
        N_CURR_LINES = findLSRs(LSR_c, I_c);
        Line *Lines_c = new Line[N_CURR_LINES];   
        linesFromLSRs(Lines_c, LSR_c, 0);   

        LineGroup L_c (Lines_c, 0);
        delete[] LSR_c;

        drawL(L_c, log_path, cleanCurr, 0);

        cout << "Current image parsed..." << endl;

        if(TESTING) cout << "Number in LSR_g: " << N_GOAL_LINES << ", LSR_c: " << N_CURR_LINES << endl; 

    //***********************~ Mapping ~*************************//

        MH *H = new MH[N_GOAL_LINES*N_GOAL_LINES*N_GOAL_LINES]; 
        int n_MHs = findMHs(H, L_g, L_c);
        if(TESTING) cout << "Number of MHs: " << n_MHs << endl;

        GMap *G = new GMap[NUM_TO_MAP];
        int n_G = mergeMHs(H, n_MHs, G, L_g, L_c); 
        if(TESTING) cout << "Number of GMaps: " << n_G << endl;

        //int blah = getVerifiedBestMap(n_G, G, L_g, L_c, 0); //TODO: fix

        drawGMaps(log_path, n_G, G, L_g, L_c, cleanGoal, cleanCurr);

    //***************~ Recover Transformations ~*****************//

        //TODO: Put this stuff somewhere else (Mapping.cpp? Or new file?)

        GMap bestMap = G[0]; //This will not always be the best, depending on the verification step.

        double x_avg = 0.0;
        double y_avg = 0.0;
        double orient_avg = 0.0;
        double scale_avg = 0.0;

        int *order_g = bestMap.getOrder_g();
        int *order_c = bestMap.getOrder_c();
        int n_lines = bestMap.getN_lines();
        for(int i=0; i<n_lines; i++){
            Line l_gi = L_g.getLineAt(order_g[i]);
            Line l_ci = L_c.getLineAt(order_c[i]);
            int *l_gi_midPt = addrToCoords(l_gi.getPoints()[2]);
            int *l_ci_midPt = addrToCoords(l_ci.getPoints()[2]);
            double l_gi_orient = l_gi.getOrientation();
            double l_ci_orient = l_ci.getOrientation();
            x_avg += l_gi_midPt[0]-l_ci_midPt[0];        
            y_avg += l_gi_midPt[1]-l_ci_midPt[1];
            double d_orient = l_gi_orient-l_ci_orient;
            if(abs(d_orient)>PI/2){
                l_gi_orient += (l_gi_orient > l_ci_orient ? (-1*PI) : PI); 
                d_orient = l_gi_orient - l_ci_orient;
            }
            if(TESTING) cout << "Goal: " << l_gi_orient << ", and Curr: " << l_ci_orient << "; d_orient = " << d_orient << endl;
            orient_avg += d_orient;
            scale_avg += l_gi.getLength()/l_ci.getLength();
        }
        x_avg /= n_lines;
        y_avg /= n_lines;
        orient_avg /= n_lines;
        scale_avg /= n_lines;

        char transform[256];

        sprintf(transform, "X shift = %f, Y shift = %f, Theta = %f, Scale = %f", x_avg, y_avg, orient_avg, scale_avg );

        /*Mat textedOut = imread("../outputImgs/mapping0.jpg"); //TODO: Loop this for all maps? I would need to make the calculations for that too.
        putText(textedOut, transform, Point(5, 475), FONT_HERSHEY_PLAIN, 1, Scalar(0,0,0));
        imwrite("texted.jpg", textedOut);    */

        cout << "X shift = " << x_avg << ", Y shift = " << y_avg 
             << ", Theta = " << orient_avg << ", Scale = " 
             << scale_avg << endl;
    

        //TODO: What did I do here?
        double distToGoal = 2; //meters
        double distToCurr = distToGoal*scale_avg;
        double viewingAngle = 30;
        double turnVal = (viewingAngle*x_avg)/(XDIM/2);

        //TODO: Write file w/ info

        if(!TESTING){
            //send data
            if (send(new_fd, "Directions...", 13, 0) == -1)
                perror("send");
        } else
            break;

//********************~ Ending main while loop ~**********************//
        cout << "Gone through loop " << loopcounter+1 << " times.\n" << endl;
        ++loopcounter;

    }

    if(TESTING){
        int t_stop = clock();
        cout << "execution time: " << (t_stop-t_start)/double(CLOCKS_PER_SEC)*1000 << endl;
    }
    return 0;
}










